/**
 * @file MapPoseCheckPlan.h
 * @author  
 * @brief map pose check
 * @version 1.0
 * @date 2020-07-23
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#pragma once

#include "rclcpp/rclcpp.hpp"
#include "nav2_msgs/srv/get_map_nav2.hpp"
#include "visualization_msgs/msg/marker_array.hpp"

#include <lib/ipa_room_planning.h>

namespace behavior_controller
{
class MapPoseCheckPlan
{
public :
    MapPoseCheckPlan() { ; };

    void SetNode(const rclcpp::Node::SharedPtr &ros_node, std::shared_ptr<IpaRoomPlanning> &ipa_room_plan_);

    void run(double &linear_vel, double &angular_vel);

    void callback(const visualization_msgs::msg::MarkerArray::SharedPtr msg);

    void subscribeAgain();

    bool is_finish() { return finished; };

    bool is_online() { return online_map; };
private:
    rclcpp::Node::SharedPtr ros_node_;
    rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr constraint_list_sub;
    std::shared_ptr<nav2_msgs::srv::GetMapNav2_Response> srv_goal = nullptr;
    std::shared_ptr<IpaRoomPlanning> ipa_room_plan = nullptr;
    bool finished = false;
    bool online_map = false;
    bool constraint_flag = false;

};

//static MapPoseCheckPlan map_pose_check_planner; // 实例化自检类
extern MapPoseCheckPlan map_pose_check_planner;   // 实例化自检类

}